#ifndef __TOY__MATH__H__
#define __TOY__MATH__H__

#define PI 3.14159
#include <iostream>

namespace ToyCube {

template <size_t COL, typename T>
struct Vec{
    T m[COL];
    
    Vec(){
        for(size_t i=0; i<COL; i++){
            m[i] = 0;
        }
    }
    
    Vec(const Vec<COL, T> &vec){
        for(size_t i=0; i<COL; i++)
            m[i] = vec[i];
    }
    
    Vec(const std::initializer_list<T> &data){
        auto it = data.begin();
        for(size_t i=0; i<COL; i++){
            m[i] = *it; it++;
        }
    }
    //读取
    const T& operator[](size_t index) const{
        return m[index];
    }
    
    //赋值
    T& operator[](size_t index) {
        return m[index];
    }
    
    Vec<COL, T>& operator=(const Vec<COL, T> vec){
        for(size_t i=0; i<COL; i++)
            m[i] = vec[i];
        return *this;
    }
    
    const Vec<COL, T>& operator*=(T k){
        for(size_t i=0; i<COL; i++)
            m[i] *= k;
        return *this;
    }
    
    void Print(){
        std::cout<<"[ ";
        for(size_t i=0; i<COL; i++)
            std::cout<<m[i]<<" ";
        std::cout<<"]"<<std::endl;
    }
};

template <size_t COL, typename T>
const Vec<COL, T> operator*(const Vec<COL, T> &vec, const T k){
    Vec<COL, T> ret;
    for(size_t i=0; i<COL; i++)
        ret[i] = vec[i] * k;
    return ret;
}

template <size_t COL, typename T>
const Vec<COL, T> operator/(const Vec<COL, T> &vec, const T k){
    Vec<COL, T> ret;
    for(size_t i=0; i<COL; i++)
        ret[i] = vec[i] / k;
    return ret;
}

template <size_t COL, typename T>
Vec<COL, T> operator+(const Vec<COL, T>& vec1, const Vec<COL, T>& vec2){
    Vec<COL, T> ret;
    for(size_t i=0; i<COL; i++)
        ret[i] = vec1[i] + vec2[i];
    return ret;
}

template <size_t COL1, size_t COL2, typename T>
Vec<COL1, T> operator+(const Vec<COL1, T>& vec1, const Vec<COL2, T>& vec2){
    Vec<COL1, T> ret;
    for(size_t i=0; i<COL2; i++)
        ret[i] = vec1[i] + vec2[i];
    return ret;
}

template <size_t COL, typename T>
Vec<COL, T> operator-(const Vec<COL, T>& vec1, const Vec<COL, T>& vec2){
    Vec<COL, T> ret;
    for(size_t i=0; i<COL; i++)
        ret[i] = vec1[i] - vec2[i];
    return ret;
}

template <size_t COL, typename T>
Vec<COL, T> Normalize(const Vec<COL, T> &vec){
    Vec<COL, T> ret;
    T totle = 0;
    for(size_t i=0; i<COL; i++){
        totle += vec.m[i] * vec.m[i];
    }
    T len = sqrt(totle);
    for(size_t i=0; i<COL; i++){
        ret.m[i] =  vec.m[i] / len;
    }
    return ret;
}

template <size_t COL, typename T>
Vec<COL, T> SubVec(const Vec<COL, T> &vec1, const Vec<COL, T> &vec2){
    Vec<COL, T> ret;
    for(size_t i=0; i<COL; i++){
        ret.m[i] = vec1.m[i] - vec2.m[i];
    }
    return ret;
}

template <size_t COL, typename T>
T DotMutVec(const Vec<COL, T> &vec1, const Vec<COL, T> &vec2){
    T ret;
    for(size_t i=0; i<COL; i++){
        ret += vec1.m[i] * vec2.m[i];
    }
    return ret;
}

template <size_t COL, typename T>
Vec<COL, T> CwiseProduct(const Vec<COL, T> &vec1, const Vec<COL, T> &vec2){
    Vec<COL, T> ret;
    for(size_t i=0; i<COL; i++){
        ret[i] = vec1.m[i] * vec2.m[i];
    }
    return ret;
}

/**
 向量叉积
 1.(二维向量叉积) 应用大小为两向量组成平行四边形的面积，用于插值计算权重
 2.(二维向量叉积) 三角形中的点与三顶点作为向量与其右边的边(右手系下)，依次叉乘，如果方向相同说明点在三角形内部
 3.(三维向量叉积) 计算相机矩阵时，已知两个向量，求与这两个向量张成平面垂直的向量
 */
template <typename T>
T CrossMutVec2(const Vec<2, T> &vec1, const Vec<2, T> &vec2){
    T ret = vec1.m[0] * vec2.m[1] - vec1.m[1]*vec2.m[0];
    return ret;
}

template <typename T>
Vec<3, T> CrossMutVec3(const Vec<3, T> &vec1, const Vec<3, T> &vec2){
    Vec<3, T> ret({
        vec1.m[1] * vec2.m[2] - vec1.m[2]*vec2.m[1],
        vec1.m[2] * vec2.m[0] - vec1.m[0]*vec2.m[2],
        vec1.m[0] * vec2.m[1] - vec1.m[1]*vec2.m[0],
    });
    return ret;
}

template <size_t ROW, size_t COL, typename T>
struct Mat{
    T m[ROW][COL];
    
    Mat(){
    }
    
    Mat(const Mat &mat){
        for(size_t i=0; i<ROW; i++){
            for(size_t j=0; j<COL; j++){
                m[i][j] = mat.m[i][j];
            }
        }
    }
    
    Mat(const std::initializer_list<Vec<COL, T>> &vecs){
        auto it = vecs.begin();
        for(size_t i=0; i<ROW; i++){
            SetRow(i, *it++);
        }
    }
    
    void SetRow(size_t row, const Vec<COL, T> &vec){
        for(size_t i=0; i<COL; i++){
            m[row][i] = vec[i];
        }
    }
    
    void SetIdentify(){
        for(size_t i=0; i<ROW; i++){
            for(size_t j=0; j<COL; j++){
                i == j? m[i][j] = 1 : m[i][j] = 0;
            }
        }
    }
    
    void Transpose(){
        for(size_t i=0; i<ROW; i++){
            for(size_t j=0; j<i; j++){
                std::swap(m[i][j], m[j][i]);
            }
        }
    }

    void RightMut(Mat<ROW, COL, T> &right){
        
        T tmp[ROW][COL];
        InitInner(tmp);
        //i : 左边行索引
        for(size_t i=0; i<ROW; i++){
            //k : 右边列索引
            for(size_t k=0; k<COL; k++){
                T sum = 0;
                //左边每行 分别乘以 右边每列
                for(size_t j=0; j<COL; j++){
                    sum += tmp[i][j] * right.m[j][k];
                }
                m[i][k] = sum;
            }
        }
    }
    
    void InitInner(T dst[][COL]){
        for(size_t i=0; i<ROW; i++){
            for(size_t j=0; j<COL; j++){
                dst[i][j] = m[i][j];
            }
        }
    }
    
    void Print(){
        for(size_t i=0; i<ROW; i++){
            std::cout<<"[ ";
            for(size_t j=0; j<COL; j++){
                std::cout<< m[i][j] <<" ";
            }
            std::cout<<"]"<<std::endl;
        }
    }
};

inline int Mid3(int data, int min, int max){
    if(data < min) data = min;
    else if(data > max) data = max;
    return data;
}

}

#endif
